#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
import rclpy.qos as qos
from sls_msgs.msg import LlaVelocity
import math
import serial

class RtkInfo(Node):
    def __init__(self, name):
        super().__init__(name)

        self.yaw_cache = None

        self.publisher = self.create_publisher(LlaVelocity, "sensors/rtk/out/lla", qos.qos_profile_sensor_data)
        self.pub_clk = self.create_timer(0.1, self.read_serial)
        self.declare_parameter("RTK_serial_port","/dev/ttyUSB0")
        RTK_serial_port = self.get_parameter("RTK_serial_port").get_parameter_value().string_value
        self.serial = serial.Serial(RTK_serial_port, 115200, timeout=0.05)

    def read_serial(self):
        if self.serial.is_open:
            data = self.serial.readline().strip().decode("ascii")
            if data.startswith("$GPRMC"):
                parts = data.split(",")
                if len(parts) >= 9 and parts[0] == "$GPRMC" and parts[2] == "A":
                    # 提取经度和纬度
                    lat = float(parts[3][0:2]) + float(parts[3][2:]) / 60.0
                    lon = float(parts[5][0:3]) + float(parts[5][3:]) / 60.0
                    alt = 0
                    # knot -> kph -> m/s
                    vel = float(parts[7]) * 1.852 / 3.6
                    # 0~360 deg from North, clockwise
                    vel_heading = float(parts[8])
                    # lon lat alt vel heading
                    yaw = 0 if self.yaw_cache is None else self.yaw_cache

                    # private protocol
                    msg = LlaVelocity()
                    msg.longitude = float(lon)
                    msg.latitude = float(lat)
                    msg.altitude = float(alt)
                    msg.vel = float(vel)
                    msg.vel_heading = float(vel_heading)
                    msg.yaw = float(yaw)
                    self.publisher.publish(msg)

                    self.get_logger().info(
                        f"(lat, lon, alt): ({lat:3>6.7f}, {lon:0>3.7f}, {alt:0>3.7f}), vel: {vel:0>2.2f}, vel_heading: {vel_heading:0>3.2f}, yaw: {yaw:0>3.2f}"
                    )
            elif data.startswith("#HEADING2A"):
                data2 = data.split(";")[1]
                if data2.startswith("SOL_COMPUTED"):
                    parts = data2.split(",")
                    self.yaw_cache = float(parts[3])
                    self.get_logger().debug(f"yaw: {self.yaw_cache:0>6.2f}")
                else:
                    self.get_logger().info("get INSUFFICIENT_OBS")
            else:
                # empty
                pass


def main():
    rclpy.init()
    rclpy.spin(RtkInfo(name="rtk_position"))
    rclpy.shutdown()
